#include "collision_detection_kernels.h"

#include <device_launch_parameters.h>
#include <device_functions.h>
#include <cmath>

#include <geometry\primitive.h>

namespace gc {
	
	// using reordered depth-first layout
	__global__ void genBroadPhaseCP(int leafSize, LBvhLeaves leaves, LBvhInteriors interiors, CollisionPair *cpQueue, int *cpSize) {
		int idx = blockIdx.x*blockDim.x + threadIdx.x;
		if (idx >= leafSize-1) return;
		//if (idx != 10) return;

		int node_i = idx; //leaf idx
		int primIdx_i = leaves.primIdx[idx];
		const BV bv_i = leaves.bv.getBV(node_i);
		//printf("%f %f %f\n", bv_i.ptMin.x, bv_i.ptMin.y, bv_i.ptMin.z);
		

		// do sprouting
		int L = 0, R = 0;
		bool overlap = false;
		int interior_idx = 0;
		//BV t;
		for (int begin = idx + 1; begin < leafSize; begin++) {
			//printf("visit leaf %d\n", begin);
			L = leaves.segOffset[begin];
			//R = L + leaves.segLen[begin];
			R = begin == leafSize - 1 ? L : leaves.segOffset[begin + 1];
			//R = leaves.parent[begin];
			overlap = true;
			//if (R - L >= 6) {
			for (interior_idx = L; interior_idx < R; ++interior_idx) {
				//printf("visit interior %d\n", interior_idx);
				//if (interior_idx<0 || interior_idx >= leafSize - 1) printf("interior_idx: %d\n", interior_idx);
				//t = interiors.bv.getBV(interior_idx);
				//overlap = bv_i.overlap(t);
				overlap = interiors.bv.overlap(interior_idx, bv_i);
				//overlap = interiors.bv[interior_idx].overlap(bv_i);
				if (!overlap) {
					begin = interiors.rangeR[interior_idx];
					break;
				};
			}
			//}
			if (!overlap) continue;

			if (leaves.bv.overlap(begin, bv_i)) {
				if (leaves.bv.overlap(node_i, begin)) {
					int primIdx_j = leaves.primIdx[begin];
					CollisionPair cp{ primIdx_i,primIdx_j };
					cpQueue[atomicAdd(cpSize, 1)] = cp;
				}
			}
		}
		
	}
	
	__global__ void brutalForce(int primSize, BV_SOA bvs, CollisionPair *cpQueue, int *cpSize) {
		int idx = blockIdx.x*blockDim.x + threadIdx.x;
		if (idx >= primSize - 1) return;
		//if (idx != 10) return;

		for (int j = idx + 1; j < primSize; ++j) {
			if (bvs.overlap(idx,j)) {
				CollisionPair cp{ idx, j };
				cpQueue[atomicAdd(cpSize, 1)] = cp; // TODO: possible accel by storing into separate pools
			}
		}
	}
}
